#import os, sys

import pygame
if not pygame.font: print 'Warning, PyGame fonts not imported. They will be disabled.'
if not pygame.mixer: print 'Warning, PyGame sounds not imported. They will be disabled.'

from OpenGL.GL import *
from OpenGL.GLU import *

from Box2D2 import *

from World import *
from Renderer import *
from Input import *
from GUI import *
from Timing import *
from Debugging import *
from Rules import *

import xml.dom.minidom as xmlHandler

#import threading
#import thread

# TODO: Think about whether placing entity management in the Level object could
#       be a good idea.

# TODO: Write decent implementations of Vec2, Mat22 (etc...) and XForm that
#       seamlessly interoperate with the Box2D versions, iterables, and OpenGL.

# TODO: Once got everything to a decent stage, consider writing on forums about
#       changes\additions we made to Box2D that we felt were necessary and feel
#       would be useful to others.
#       e.g., XForm --> OpenGL transformation matrix, Vec2 & Mat22 augmentations

# Fonts (textures or geometric)
# Engine attachments for GUI items and cyclic worlds

# Proper air resistance for box2D
# Portals

#TODO: Decide on exception class distinctions
class Error(Exception):
    pass
class EngineError(Error):
    pass

class Engine(object):
    """
    class __EventListener:
        #def __init__(self, listeningObject, callbackFunc):
        def __init__(self, callbackFunc):
            #self.object = listeningObject
            self.callback = callbackFunc
        def inform(self, *args):
            argList = list(args)
            #if self.object is not None:
            #    argList.insert(0, self.object)
            self.callback(*argList)
    """
            
    def __init__(self):
        # Default settings
        self.tickRate = 60
        self.gravity = -5.0
        self.worldMinX = -20.0
        self.worldMinY = -20.0
        self.worldMaxX = 20.0
        self.worldMaxY = 20.0
        # TODO: Work out why turning this on makes the physics go haywire
        self.usePhysicsSleeping = False
        self.windowWidth = 1000
        self.windowHeight = 700
        self.forceResolution = False
        self.fullscreen = False
        #self.physicsIterations = 10
        self.velocity_iterations = 10
        self.position_iterations = 8
        
        self.renderTime = 1.0 / 30
            
        # Dict of events and their listeners
        #self.eventListeners = {'tick': [], 'input': [], 'renderGUI': [], 'render': []}

        self.event_handlers = {}
    """
    #def addEventListener(self, eventName, listener):
    #    self.eventListeners[eventName].append(listener)
        
    #def addEventListener(self, eventName, listeningObject, callbackFunc):
    def addEventListener(self, eventName, callbackFunc):
        #self.eventListeners[eventName].append( self.__EventListener(listeningObject, callbackFunc) )
        self.eventListeners[eventName].append( callbackFunc )
        
    #def addEventListener(self, object):
    #    pass
        #if object.
    """

    # Note: This method means the event handlers and the events are very
    #       decoupled, which I don't really like. The function signatures
    #       must match, but they are not known until runtime.
    def appendEventHandler(self, event, handler):
        if event not in self.event_handlers.keys():
            self.event_handlers[event] = [handler]
        else:
            self.event_handlers[event].append(handler)
    """
    def event_handler(self, event):
        def decorator(func):
            def replacement(*args, **kwargs):
                return func(*args, **kwargs)
            if event not in self.event_handlers.keys():
                self.event_handlers[event] = [func]
            else:
                self.event_handlers[event].append(func)
            return replacement
        return decorator
    """

    def raiseEvent(self, event, *args, **kwargs):
        if event in self.event_handlers.keys():
            for handler in self.event_handlers[event]:
                ret = handler(*args, **kwargs)
                if not ret:
                    break

    # TODO: Decide whether to keep this
    """
    def raiseEvent(self, eventName, *args):
        for listener in self.eventListeners[eventName]:
            #listener.inform(self, *args)
            #if hasattr() ?
            listener(self, *args)
            # TODO: Fix the exception handling so that it doesn't
            #        catch errors within the function listener( )
            try:
                listener(self, *args)
            except (TypeError), error:
                # TODO: Work out better way to show where the real problem is
                #error.args = (error.args[0] + "\nYour callback function does not match the signature of the event",)
                argList = inspect.getargspec(listener)[0][0]
                for arg in inspect.getargspec(listener)[0][1:]:
                    argList += ", " + arg
                raise EngineError( "Callback "+str(listener.im_class)+'.'+listener.__name__+"(" + argList + ") doesn't match event signature. Needs to accept self+"+str(1+len(args))+" arg(s)." )
            #if listener.object == None:
            #    # Inform simple function listener of event
            #    listener.callback(self, *args)
           #else:
            #    # Inform member function listener of event
            #    listener.callback(listener.object, self, *args)
    """

    def init(self):
        
        # Custom settings loaded from file
        #settingsFile = file("settings.txt", "r")
        #for line in settingsFile:
        #    bits = line.split(" = ")
        #    self.__dict__[bits[0]] = type(self.__dict__[bits[0]])(bits[1])
        #    print "Loaded setting ", bits[0], " = ", bits[1]
        
        pygame.init()
        
        self.clock = pygame.time.Clock()
        self.input = Input()
        self.renderer = Renderer()
        self.renderer.init(self.windowWidth, self.windowHeight, self.fullscreen, self.forceResolution)
        # TODO: Check what self.clock is and how it relates to TimeMgr
        self.timeMgr = TimeMgr()
        self.timeMgr.init()

        self.world = World(self)
        
        #Set up and create the physics world
        aabbWorld = b2AABB()
        aabbWorld.lowerBound.Set(self.worldMinX, self.worldMinY)
        aabbWorld.upperBound.Set(self.worldMaxX, self.worldMaxY)
        gravity = b2Vec2(0, self.gravity)
        self.physics_world = b2World(aabbWorld, gravity, self.usePhysicsSleeping)

        self.mouseJoint = None
        self.gui = GUI(self)
        
        self.bodyLoader = BodyLoader(self.physics_world)
        self.animationLoader = AnimationLoader()
        self.animationLoader.loadResources()
        
        self.running = False

        # TODO: Replace this with entity\event\etc. manager
        self.entities = {}
        
        self.time_accel = 0.0
        
        # ...
    
    def run(self):
        tickDuration = 1.0 / float(self.tickRate)
        self.running = True
        tickCount = 0
        frameCount = 0
        lastSecond = time.clock()
        lastIterationTime = time.clock()
        while self.running:
            
            # Sweet main loop that ticks at self.tickRate and renders as
            # fast as possible, with no wasted time. And so simple!
            # The only limitation is that at frame rate > tick rate, the
            # extra frames will not make any visual difference unless we
            # add interpolation before rendering. But our tick rate must
            # be 60 anyway, and that's more than high enough.
            
            tickDuration = 1.0 / float(self.tickRate) * (1.0 - self.time_accel)
            
            now = time.clock()
            while lastIterationTime + tickDuration <= now:
                self.tick()
                tickCount += 1
                lastIterationTime += tickDuration
            self.render()
            frameCount += 1
            
            # Print the tick and frame rates averaged over the last second
            elapsedSinceLastSecond = time.clock() - lastSecond
            if elapsedSinceLastSecond > 1.0:
                print "Ticks/second: ", tickCount / elapsedSinceLastSecond
                print "Frames/second: ", frameCount / elapsedSinceLastSecond
                tickCount = 0
                frameCount = 0
                lastSecond = time.clock()
            
    def tick(self):
        
        self.input.update()
        self.handleOSEvents()
        #if self.input.keyIsDown(pygame.K_a):
        #    self.running = False
        if self.input.mouseButtonIsDown(2):
            pass
            #mousePos = self.input.getMousePos()
            #mousePos = self.convertWindowCoordsToWorld(mousePos)
            #self.renderer.camera.move(mousePos)
        if self.input.keyIsDown(pygame.K_p):
            self.time_accel = 0.25
        elif self.input.keyIsDown(pygame.K_l):
            self.time_accel = -0.25
        else:
            self.time_accel = 0.0

        # TODO: Remove this when possible
        #self.raiseEvent('tick', 1.0 / self.tickRate)
        
        # Perform physics updates
        self.physics_world.Step(1.0 / self.tickRate, self.velocity_iterations, self.position_iterations)
        
        self.world.update(1.0 / self.tickRate)
        
        # *...?* What if entities were replaced with objects that could add
        #        listeners to engine events. Each engine event (onTick,
        #        onEvent, onRenderGUI, etc...) would have an associated list
        #        of listeners which would each be informed upon activation.
        #        Almost like sets of rules. Entities would be like
        #        extensions of the engine.
        
    def render(self):

        self.renderer.camera.update(1.0)
        
        # TODO: Remove this as I am beginning to think it's crap
        #self.raiseEvent('render')

        # TODO: Remove this as it's been replaced with renderer-based model-
        #       management.
        #self.world.draw()

        """
        #Render the world extents
        glBegin(GL_LINE_LOOP)
        glColor4f(1, 1, 1, 0.8)
        glVertex3f(self.worldMinX, self.worldMinY, 0)
        glVertex3f(self.worldMaxX, self.worldMinY, 0)
        glVertex3f(self.worldMaxX, self.worldMaxY, 0)
        glVertex3f(self.worldMinX, self.worldMaxY, 0)
        glEnd()
        #Render the ground body
        self.renderDebugBody(self.world.GetGroundBody())
        """
        
        #for entity in self.entities.values():
            #entity.render()
        
        # TODO: Replace this with simply making GUI elements entities that are
        #       rendered in the GUI layer.
        #self.raiseEvent('renderGUI')
        
        self.renderer.render()

    # TODO: Replace x and y with a proper vector type
    def queryEntitiesAtPoint(self, x, y):
        """Returns a list of entities that contain physical shapes that
           intersect the given point.
        """

        pos = b2Vec2(x, y)
        
        # Make a small box
        aabb = b2AABB()
        min_size = b2Vec2(0.001, 0.001)
        aabb.lowerBound = pos - min_size;
        aabb.upperBound = pos + min_size;
        # Query the world for overlapping shapes.
        # TODO: Check this
        max_count = 10
        body = None
        count, shapes = self.physics_world.Query(aabb, max_count)
        entities = []
        # Build a list of entities from the list of shapes
        for shape in shapes:
            entities.append(shape.GetBody().GetUserData())
        return entities
    def queryEntitiesInRect(self, x1, y1, x2, y2):
        """Returns a list of entities that contain physical shapes that
           intersect the given rectangle.
        """
        # Make a small box
        aabb = b2AABB()
        min_size = b2Vec2(0.001, 0.001)
        # Check point order
        if x2 < x1:
            x1, x2 = x2, x1
        if y2 < y1:
            y1, y2 = y2, y1
        # Check minimum rect size
        if x2-x1 < 2*min_size.x:
            x1 -= min_size.x
            x2 += min_size.x
        if y2-y1 < 2*min_size.y:
            y1 -= min_size.y
            y2 += min_size.y
        aabb.lowerBound = b2Vec2(x1, y1);
        aabb.upperBound = b2Vec2(x2, y2);
        # Query the world for overlapping shapes.
        # TODO: Check this
        max_count = 100
        count, shapes = self.physics_world.Query(aabb, max_count)
        entities = []
        # Build a list of entities from the list of shapes
        for shape in shapes:
            entities.append(shape.GetBody().GetUserData())
        return entities
    """
@the_engine.responseToEvent("mouse_button_down")
def onMouseButtonDown(self, button, world_pos):
    entities = the_engine.queryPoint(world_pos)
    if button == 0:
        pass
        # ...
    
def handleOSEvents(self):
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            self.raiseEvent("quit")
            #self.running = False
        elif event.type == pygame.KEYDOWN
            self.raiseEvent("key_down", key=event.key, modifiers=event.mod)
        elif event.type == pygame.KEYUP
            self.raiseEvent("key_up", key=event.key, modifiers=event.mod)
        elif event.type == pygame.MOUSEBUTTONDOWN:
            # TODO: Think how to handle this nicely
            world_pos = transformPxl2World(event.pos)
            gui_pos = transformPxl2GUI(event.pos)
            self.raiseEvent("mouse_button_down", button=event.button, \
                            world_pos=world_pos, gui_pos=gui_pos)
    """
    def handleOSEvents(self):
        for event in pygame.event.get():
            #if hasattr(event, 'pos'):
                #event.pos = self.convertPxlsToWorldCoords(event.pos)
            self.raiseEvent("pygame_event", event)
            if event.type == pygame.KEYDOWN:
                #self.raiseEvent("key", action="down", key=event.key, \
                #                modifiers=event.mod)
                self.raiseEvent("key", "down", event.key, event.mod)
            elif event.type == pygame.KEYUP:
                self.raiseEvent("key", "up", event.key, event.mod)
            elif event.type == pygame.MOUSEBUTTONDOWN:
                # TODO: Think how to handle this nicely
                world_pos = event.pos#transformPxl2World(event.pos)
                gui_pos = event.pos#transformPxl2GUI(event.pos)
                self.raiseEvent("mouse_button", "down", event.button, \
                                world_pos, gui_pos)
            elif event.type == pygame.MOUSEBUTTONUP:
                # TODO: Think how to handle this nicely
                world_pos = event.pos#transformPxl2World(event.pos)
                gui_pos = event.pos#transformPxl2GUI(event.pos)
                self.raiseEvent("mouse_button", "up", event.button, \
                                world_pos, gui_pos)
            elif event.type == pygame.MOUSEMOTION:
                # TODO: Think how to handle this nicely
                world_pos = event.pos#transformPxl2World(event.pos)
                gui_pos = event.pos#transformPxl2GUI(event.pos)
                # TODO: Do this properly
                rel = event.rel#transformPxl2World(event.rel)
                self.raiseEvent("mouse_motion", event.buttons, \
                                world_pos, gui_pos, rel)
            elif event.type == pygame.QUIT:
                self.raiseEvent("quit")
            """elif event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE:
                self.running = False
            elif event.type == pygame.KEYDOWN and event.key == pygame.K_SPACE:
                self.renderer.camera.rotate(1.0)
            elif event.type == pygame.KEYDOWN and event.key == pygame.K_m:
                self.renderer.toggleMotionBlur()
            elif event.type == pygame.KEYDOWN and event.key == pygame.K_UP:
                self.renderer.motionBlurFactor += 0.1
            elif event.type == pygame.KEYDOWN and event.key == pygame.K_DOWN:
                self.renderer.motionBlurFactor -= 0.1
            elif event.type == pygame.MOUSEBUTTONDOWN:
                pos = self.convertPxlsToWorldCoords(event.pos)
                pos = b2Vec2(pos[0], pos[1])
                mjd = b2MouseJointDef()
                # Make a small box.
                aabb = b2AABB()
                d = b2Vec2()
                d.Set(0.001, 0.001)
                aabb.lowerBound = pos - d;
                aabb.upperBound = pos + d;

                # Query the world for overlapping shapes.
                k_maxCount = 10
                body = None
                count, shapes = self.physics_world.Query(aabb, k_maxCount)
                for i in range(count):
                    shapeBody = shapes[i].GetBody()
                    if shapeBody.IsStatic() == False and shapeBody.GetMass() > 0.0:
                        inside = shapes[i].TestPoint(shapeBody.GetXForm(), pos)
                        if inside:
                            body = shapes[i].GetBody()
                            break
                if body is not None:
                    md = b2MouseJointDef()
                    md.body1 = self.physics_world.GetGroundBody()
                    md.body2 = body
                    md.target = pos
                    md.maxForce = 1000.0 * body.GetMass()
                    self.mouseJoint = self.physics_world.CreateJoint(md).getAsType()
                    body.WakeUp()
            elif event.type == pygame.MOUSEBUTTONUP:
                if self.mouseJoint is not None:
                    self.physics_world.DestroyJoint(self.mouseJoint)
                    self.mouseJoint = None
            elif event.type == pygame.MOUSEMOTION:
                if self.mouseJoint is not None:
                    pos = self.convertPxlsToWorldCoords(event.pos)
                    pos = b2Vec2(pos[0], pos[1])
                    self.mouseJoint.SetTarget(pos)
                    
            self.raiseEvent('input', event)
            """
    def convertPxlsToGUICoords(self, point):
        point = list(point)
        # First, scale out pixels
        size_pxls = self.renderer.getSizeInPxls()
        point[0] /= float(size_pxls[0])
        point[1] /= -float(size_pxls[1])
        # Then place the origin at the centre of the window and
        #  scale to [-1, 1] [-aspect, aspect]
        size = self.renderer.getSize()
        point[0] = (point[0] - 0.5) * size[0]
        point[1] = (point[1] + 0.5) * size[1]
        return point
    def convertPxlsToWorldCoords(self, point):
        # First convert to GUI coords
        pos = self.convertPxlsToGUICoords(point)

        # Then undo the camera transformations
        angle = math.radians(-self.renderer.camera.getAngle())
        tmp_pos = list(pos)
        tmp_pos[0] = pos[0]*math.cos(angle) - pos[1]*math.sin(angle)
        tmp_pos[1] = pos[0]*math.sin(angle) + pos[1]*math.cos(angle)
        pos = tmp_pos
        
        pos[0] /= self.renderer.camera.getZoom()
        pos[1] /= self.renderer.camera.getZoom()
        pos[0] += self.renderer.camera.getPos()[0]
        pos[1] += self.renderer.camera.getPos()[1]
        
        return pos

    # TODO: Remove this as it seems the above functions are used instead
    def convertWindowCoordsToWorld(self, pos):
        """
        TODO: Rewrite to use vectors and matrices
        """
        pos = list(pos)
        # First, scale out pixels
        pos[0] /= float(self.renderer.width)
        pos[1] /= -float(self.renderer.height)
        # Then place the origin at the centre of the window and scale to [-1, 1]
        pos[0] = (pos[0] - 0.5) * 2.0
        pos[1] = (pos[1] + 0.5) * 2.0 * self.renderer.aspect
        
        # Finally, account for the camera (transformations)
        pos[0] /= self.renderer.camera.getZoom()
        pos[1] /= self.renderer.camera.getZoom()
        pos[0] += self.renderer.camera.getPos()[0]
        pos[1] += self.renderer.camera.getPos()[1]
        
        # TODO: See if can work this out
        #rot = self.renderer.camera.xform.R
        # TODO: This should be replaced with a simple copy constructor
        #rot = b2Mat22(rot.col1, rot.col2)
        #rot.Invert()
        #posVec = b2Mul(rot, b2Vec2(pos[0], pos[1]))
        
        angle = math.radians(-self.renderer.camera.getAngle())
        tmp_pos = list(pos)
        tmp_pos[0] = pos[0]*math.cos(angle) - pos[1]*math.sin(angle)
        tmp_pos[1] = pos[0]*math.sin(angle) + pos[1]*math.cos(angle)
        
        return tmp_pos
        #return (posVec.x, posVec.y)

    # TODO: Remove or fix this as it doesn't work with the new layered rendering
    #       system.
    def renderDebugBody(self, body):
        """Crudely renders a b2Body for debugging purposes
        """
        glPushMatrix()
        xform = body.GetXForm()
        glMultMatrixf(xform.GetOpenGLForm())
        shape = body.GetShapeList()
        while shape != None:
            if shape.GetType() == e_polygonShape:
                verts = shape.asPolygon().getVertices_b2Vec2()
            elif shape.GetType() == e_circleShape:
                angleStep = 20
                verts = [ b2Vec2(shape.asCircle().GetRadius()*math.cos(theta*b2_pi/180.0), -shape.asCircle().GetRadius()*math.sin(theta*b2_pi/180.0)) for theta in range(359, 0, -angleStep) ]
            # No texturing
            glBindTexture(GL_TEXTURE_2D, 0)
            # Render the polygon
            glColor4f(1, 1.0, 1, 0.5)
            glBegin(GL_POLYGON)
            for vert in verts:
                glVertex3f(vert.x, vert.y, 0)
            glEnd()
            # Render the outline
            glBegin(GL_LINE_LOOP)
            glColor4f(0, 1.0, 1.0, 0.5)
            for vert in verts:
                glVertex3f(vert.x, vert.y, 0)
            glEnd()
            shape = shape.GetNext()
        glPopMatrix()

class Entity(object):
    def __init__(self, engine, name):
        self.engine = engine
        self.name = name
    def update(self, timeStep):
        pass
    # TODO: Remove this completely
    #def draw(self):
    #    pass

class CameraController(Entity):
    def __init__(self, engine, name):
        Entity.__init__(self, engine, name)
    def update(self, timeStep):
        # ...
        pass

class BodyLoader(object):
    """
    Creates physics bodies from various data sources
    """
    class FullBodyDef(object):
        def __init__(self):
            #self.bodyDef = b2BodyDef()
            self.shapeDefs = []
            self.jointDefs = []
    def __init__(self, physics_world):
        self.physics_world = physics_world
        self.bodyDef = b2BodyDef()
        self.bodyDef.userData = None
        self.loadedBodies = {}
        
    def createBodyFromFile(self, filename, pos=b2Vec2(0, 0)):
        # TODO: Beware of bodyDef.fixedRotation
        self.bodyDef.position = pos
        fullBodyDef = FullBodyDef()
        if self.loadedBodies.has_key(filename):
            fullBodyDef = self.loadedBodies[filename]
        else:
            # TODO: This is just example code to show how it will work
            # ...
            # Load the shape definitions
            shapeDefs = []
            shapeDefs.append(b2PolygonDef())
            shapeDefs.append(b2CircleDef())
            fullBodyDef.shapeDefs = shapeDefs
            # Load the joint definitions
            jointDefs = []
            jointDefs.append(b2JointDef())
            jointDefs.append(b2DistanceJointDef())
            fullBodyDef.jointDefs = jointDefs
            # Store the full body definition for repeat use
            self.loadedBodies[filename] = fullBodyDef
        # Create the body
        # TODO: Check if bodyDef needs to be loaded from file as well
        body = self.physics_world.CreateBody(self.bodyDef)
        # Add the shapes
        for shapeDef in fullBodyDef.shapeDefs:
            body.CreateShape(shapeDef)
        body.SetMassFromShapes()
        for jointDef in fullBodyDef.jointDefs:
            body.CreateJoint(jointDef)
        return body
    
    def createRectBody(self, pos=b2Vec2(0, 0), size=b2Vec2(0.1, 0.1), density=1, friction=0.3, restitution=0.4, groupIndex=0, fixed_rotation=False):
        self.bodyDef.position = pos
        self.bodyDef.fixedRotation = fixed_rotation
        # Create the body
        body = self.physics_world.CreateBody(self.bodyDef)
        # Add the shape
        shapeDef = b2PolygonDef()
        shapeDef.SetAsBox(size.x, size.y)
        #for vert in shapeDef.getVertices_b2Vec2():
        #    vert += b2Vec2(0.05, 0.05)
        shapeDef.density = density
        shapeDef.friction = friction
        shapeDef.restitution = restitution
        shapeDef.filter.groupIndex = groupIndex
        body.CreateShape(shapeDef)
        shapeDef.SetAsBox(size.x, size.y)
        #body.CreateShape(shapeDef)
        body.SetMassFromShapes()
        return body
    
    def createCircleBody(self, pos=b2Vec2(0, 0), radius=0.1, density=1, friction=0.3, restitution=0.5, fixed_rotation=False):
        self.bodyDef.position = pos
        self.bodyDef.fixedRotation = fixed_rotation
        # Create the body
        body = self.physics_world.CreateBody(self.bodyDef)
        # Add the shape
        shapeDef = b2CircleDef()
        shapeDef.radius = radius
        shapeDef.density = density
        shapeDef.friction = friction
        shapeDef.restitution = restitution
        body.CreateShape(shapeDef)
        body.SetMassFromShapes()
        return body

# TODO: This is currently just used as a test hack
class Body(Entity):
    def __init__(self, engine, name, pos=b2Vec2(0.0, 0), density=1):
        Entity.__init__(self, engine, name)
        
        self.body = engine.bodyLoader.createRectBody(pos, b2Vec2(0.1, 0.1), density)
        #self.body.ApplyForce( b2Vec2(0.1, 0), b2Vec2(0.0, 0.1) )
        #self.body.ApplyTorque( 0.01 )
        
        self.model = engine.renderer.\
                     createModel(layer="play", \
                                 mesh="box", \
                                 texture="Box.jpg", \
                                 influences=[(self.body.GetXForm(), 1.0)], \
                                 local_xform=b2XForm_identity)
        #texture.startAnimating(engine)

    def __del__(self):
        self.engine.renderer.destroyModel(self.model)
        
    def update(self, timeStep):
        pass

    # TODO: Remove this completely
    #def draw(self):
    #    self.model.draw()

    # TODO: Remove this old hack   
    #def renderDebug(self):
    #    self.engine.renderDebugBody(self.body)

# TODO: See what to do with this. It's badly aged, and needs a rethink regarding
#       its purpose and use.
class CyclicWorldBody(Body):
    def __init__(self, engine, name, pos=b2Vec2(0.0, 0.0), density=1):
        Body.__init__(self, engine, name, pos, density)
        
        #Create clone bodies
        maxY = self.engine.renderer.aspect
        # Epsilon hack to avoid /0
        self.epsilon = 1.0 / 1000000000
        if abs(pos.x) < self.epsilon: pos.x = self.epsilon
        if abs(pos.y) < self.epsilon: pos.y = self.epsilon
        dx = -pos.x / abs(pos.x) * 2.0
        dy = -pos.y / abs(pos.y) * 2.0 * maxY
        self.bodyCloneX = engine.bodyLoader.createRectBody(pos + b2Vec2(dx, 0), b2Vec2(0.1, 0.1), density, groupIndex=-1)
        self.bodyCloneY = engine.bodyLoader.createRectBody(pos + b2Vec2(0, dy), b2Vec2(0.1, 0.1), density, groupIndex=-1)
        self.bodyCloneXY = engine.bodyLoader.createRectBody(pos + b2Vec2(dx, dy), b2Vec2(0.1, 0.1), density, groupIndex=-1)
    
    def update(self, timeStep):
        
        def stretchBody(body):
            # TODO: This is a big hack
            #       It could be optimised by creating (check forums for existing) a
            #       function to replace DestroyShape&CreateShape
            shapeList = body.GetShapeList()
            shape = shapeList
            self.shapeDef = b2PolygonDef()
            
            verts = shape.asPolygon().getVertices_b2Vec2()
            size = b2Vec2(verts[1].x - verts[0].x, verts[2].y - verts[1].y) * 0.5
            self.shapeDef.SetAsBox(size.x*1.001, size.y)
            self.shapeDef.density = 1.0
            self.shapeDef.friction = 0.3
            self.shapeDef.restitution = 0.4
            body.DestroyShape(shape)
            body.CreateShape(self.shapeDef)
        return
        stretchBody(self.body)
        #stretchBody(self.bodyCloneX)
        #stretchBody(self.bodyCloneY)
        #stretchBody(self.bodyCloneXY)
        
        pos = self.body.GetPosition()
        angle = self.body.GetAngle()
        
        # TODO: Unhack this by using proper world extents
        
        maxY = self.engine.renderer.aspect
        
        if pos.x > 1.0: pos.x -= 2.0
        if pos.x < -1.0: pos.x += 2.0
        if pos.y > maxY: pos.y -= 2.0 * maxY
        if pos.y < -maxY: pos.y += 2.0 * maxY
        #self.body.GetXForm().position = pos
        self.body.SetXForm(pos, angle)
        # Avoid /0
        if abs(pos.x) < self.epsilon: pos.x = self.epsilon
        if abs(pos.y) < self.epsilon: pos.y = self.epsilon
        dx = -pos.x / abs(pos.x) * 2.0
        dy = -pos.y / abs(pos.y) * 2.0 * maxY
        
        #self.bodyCloneX.GetXForm().position = pos + b2Vec2(dx, 0)
        #self.bodyCloneY.GetXForm().position = pos + b2Vec2(0, dy)
        #self.bodyCloneXY.GetXForm().position = pos + b2Vec2(dx, dy)
        self.bodyCloneX.SetXForm(pos + b2Vec2(dx, 0), angle)
        self.bodyCloneY.SetXForm(pos + b2Vec2(0, dy), angle)
        self.bodyCloneXY.SetXForm(pos + b2Vec2(dx, dy), angle)
        
        linVel = self.body.GetLinearVelocity()
        angVel = self.body.GetAngularVelocity()
        self.bodyCloneX.SetLinearVelocity(linVel)
        self.bodyCloneX.SetAngularVelocity(angVel)
        self.bodyCloneY.SetLinearVelocity(linVel)
        self.bodyCloneY.SetAngularVelocity(angVel)
        self.bodyCloneXY.SetLinearVelocity(linVel)
        self.bodyCloneXY.SetAngularVelocity(angVel)
        
    def render(self):
        Body.render(self)
        
        self.engine.renderDebugBody(self.bodyCloneX)
        self.engine.renderDebugBody(self.bodyCloneY)
        self.engine.renderDebugBody(self.bodyCloneXY)
        """
        glPushMatrix()
        xform = self.bodyCloneX.GetXForm()
        glMultMatrixf(xform.GetOpenGLForm())
        #self.model.render()
        self.renderDebug()
        glPopMatrix()
        glPushMatrix()
        xform = self.bodyCloneY.GetXForm()
        glMultMatrixf(xform.GetOpenGLForm())
        #self.model.render()
        self.renderDebug()
        glPopMatrix()
        glPushMatrix()
        xform = self.bodyCloneXY.GetXForm()
        glMultMatrixf(xform.GetOpenGLForm())
        #self.model.render()
        self.renderDebug()
        glPopMatrix()
        """

class CompositeBody(Entity):
    def __init__(self, engine, name):
        Entity.__init__(self, engine, name)
        
        self.bodies = {}
        self.joints = {}
        #TODO: Work out how to render the models. Requires thinking about how
        #      to specify their xform. Giving them an xform could improve the
        #      layer handling code.
        self.models = {}
        #self.model_list = []
        
        # The body that is referred to when querying the position or angle of the composite body
        self.ref_body = None

    def __del__(self):
        for model in self.models.values():
            self.engine.renderer.destroyModel(model)

    # TODO: Decide whether to use this and implement it
    def addModel(self, name, model):
        pass

    # Note: This must be used to ensure that the body's userData is set!
    # TODO: Consider this interface and decide whether to make self.bodies
    #       private and do the same for joints and models.
    def addBody(self, name, body, make_reference_body=False):
        body.SetUserData(self)
        if make_reference_body:
            self.ref_body = body
        self.bodies[name] = body

    def setReferenceBody(self, name):
        self.ref_body = self.bodies[name]

    def getXForm(self):
        return self.ref_body.GetXForm()
    def getPos(self):
        return self.ref_body.GetPosition()
    def getAngle(self):
        return self.ref_body.GetAngle()
    def getLinearVelocity(self):
        return self.ref_body.GetLinearVelocity()
    def getAngularVelocity(self):
        return self.ref_body.GetAngularVelocity()
    
    def setXForm(self, newXForm):
        # TODO: Check that this works
        posChange = newXForm.position - self.ref_body.GetWorldCenter()
        angleChange = newXForm.R.GetAngle() - self.ref_body.GetAngle()
        for body in self.bodies.values():
            body.SetXForm(body.GetWorldCenter() + posChange, \
                          body.GetAngle() + angleChange)
            
    def applyForce(self, force, pos=None):
        if pos == None:
            pos = self.ref_body.GetPosition()
        for body in self.bodies.values():
            body.ApplyForce(force, pos)

    # TODO: This is deprecated in favour of applyForce with no second arg
    def applyCenteredForce(self, force):
        for body in self.bodies.values():
            body.ApplyForce(force, body.GetWorldCenter())

    def applyTorque(self, torque):
        for body in self.bodies.values():
            body.ApplyTorque(torque)
            
    def applyDampingForce(self, force):
        for body in self.bodies.values():
            body.ApplyForce(force * -body.GetLinearVelocity(), body.GetWorldCenter())
    def applyDampingTorque(self, torque):
        for body in self.bodies.values():
            body.ApplyTorque(torque * -body.GetAngularVelocity())

    # TODO: Remove this completely
    #def draw(self):
    #    for model in self.models.values():
    #        # Note: This applies no xform to the rendering of the models!
    #        model.draw()

class CharacterBody(CompositeBody):
    def __init__(self, engine, name, refPos, refAngle=0.0):
        CompositeBody.__init__(self, engine, name)
        
        bodyDef = b2BodyDef()
        bodyDef.position = refPos
        bodyDef.angle = refAngle

        revJointDef = b2RevoluteJointDef()
        # Enable 'friction' in the joint
        revJointDef.enableMotor = True
        revJointDef.motorSpeed = 0.0
        revJointDef.maxMotorTorque = 0.9
        
        circDef = b2CircleDef()
        circDef.density = 1.0
        circDef.filter.groupIndex = -2
        polyDef = b2PolygonDef()
        polyDef.density = 1.0
        polyDef.filter.groupIndex = -2
        
        bodyDef.position = refPos + b2Vec2(0.0, 0.65)
        body = self.bodies["Head"] = engine.physics_world.CreateBody(bodyDef)
        circDef.radius = 0.1
        body.CreateShape(circDef)
        body.SetMassFromShapes()
        
        bodyDef.position = refPos + b2Vec2(0.0, 0.54)
        body = self.bodies["Neck"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.05, 0.04)
        body.CreateShape(polyDef)
        body.SetMassFromShapes()
        
        revJointDef.Initialize(self.bodies["Head"], self.bodies["Neck"], refPos + b2Vec2(0.0, 0.58))
        revJointDef.enableLimit = True
        revJointDef.lowerAngle = b2_pi/180.0 * -10
        revJointDef.upperAngle = b2_pi/180.0 * 20
        self.joints["Head"] = engine.physics_world.CreateJoint(revJointDef)
        
        bodyDef.position = refPos + b2Vec2(0.0, 0.3)
        body = self.bodies["UpperTorso"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.10, 0.2)
        body.CreateShape(polyDef)
        body.SetMassFromShapes()
        
        revJointDef.Initialize(self.bodies["Neck"], self.bodies["UpperTorso"], refPos + b2Vec2(0.0, 0.5))
        revJointDef.enableLimit = True
        revJointDef.lowerAngle = b2_pi/180.0 * -10.0
        revJointDef.upperAngle = b2_pi/180.0 * 30.0
        self.joints["Neck"] = engine.physics_world.CreateJoint(revJointDef)

        bodyDef.position = refPos + b2Vec2(0.0, 0.3)
        body = self.bodies["LeftUpperArm"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.06, 0.15)
        body.CreateShape(polyDef)
        body.SetMassFromShapes()
        
        revJointDef.Initialize(self.bodies["UpperTorso"], self.bodies["LeftUpperArm"], refPos + b2Vec2(0.0, 0.45))
        revJointDef.enableLimit = False
        revJointDef.lowerAngle = b2_pi/180.0 * -10.0
        revJointDef.upperAngle = b2_pi/180.0 * 30.0
        self.joints["LeftShoulder"] = engine.physics_world.CreateJoint(revJointDef)

        bodyDef.position = refPos + b2Vec2(0.0, 0.0)
        body = self.bodies["LeftLowerArm"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.06, 0.15)
        body.CreateShape(polyDef)
        body.SetMassFromShapes()
        
        revJointDef.Initialize(self.bodies["LeftUpperArm"], self.bodies["LeftLowerArm"], refPos + b2Vec2(0.0, 0.15))
        revJointDef.enableLimit = True
        revJointDef.lowerAngle = b2_pi/180.0 * 0.0
        revJointDef.upperAngle = b2_pi/180.0 * 135.0
        self.joints["LeftElbow"] = engine.physics_world.CreateJoint(revJointDef)
        
        bodyDef.position = refPos + b2Vec2(0.0, -0.22)
        body = self.bodies["LeftHand"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.06, 0.07)
        body.CreateShape(polyDef)
        body.SetMassFromShapes()
        
        revJointDef.Initialize(self.bodies["LeftLowerArm"], self.bodies["LeftHand"], refPos + b2Vec2(0.0, -0.15))
        revJointDef.enableLimit = True
        revJointDef.lowerAngle = b2_pi/180.0 * -35.0
        revJointDef.upperAngle = b2_pi/180.0 * 5.0
        self.joints["LeftWrist"] = engine.physics_world.CreateJoint(revJointDef)
        
        bodyDef.position = refPos + b2Vec2(0.0, 0.3)
        body = self.bodies["RightUpperArm"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.06, 0.15)
        body.CreateShape(polyDef)
        body.SetMassFromShapes()
        
        revJointDef.Initialize(self.bodies["UpperTorso"], self.bodies["RightUpperArm"], refPos + b2Vec2(0.0, 0.45))
        revJointDef.enableLimit = False
        revJointDef.lowerAngle = b2_pi/180.0 * -10.0
        revJointDef.upperAngle = b2_pi/180.0 * 30.0
        self.joints["RightShoulder"] = engine.physics_world.CreateJoint(revJointDef)

        bodyDef.position = refPos + b2Vec2(0.0, 0.0)
        body = self.bodies["RightLowerArm"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.06, 0.15)
        body.CreateShape(polyDef)
        body.SetMassFromShapes()
        
        revJointDef.Initialize(self.bodies["RightUpperArm"], self.bodies["RightLowerArm"], refPos + b2Vec2(0.0, 0.15))
        revJointDef.enableLimit = True
        revJointDef.lowerAngle = b2_pi/180.0 * 0.0
        revJointDef.upperAngle = b2_pi/180.0 * 135.0
        self.joints["RightElbow"] = engine.physics_world.CreateJoint(revJointDef)
        
        bodyDef.position = refPos + b2Vec2(0.0, -0.22)
        body = self.bodies["RightHand"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.06, 0.07)
        body.CreateShape(polyDef)
        body.SetMassFromShapes()
        
        revJointDef.Initialize(self.bodies["RightLowerArm"], self.bodies["RightHand"], refPos + b2Vec2(0.0, -0.15))
        revJointDef.enableLimit = True
        revJointDef.lowerAngle = b2_pi/180.0 * -35.0
        revJointDef.upperAngle = b2_pi/180.0 * 5.0
        self.joints["RightWrist"] = engine.physics_world.CreateJoint(revJointDef)
        
        bodyDef.position = refPos + b2Vec2(0.0, 0.0)
        bodyDef.fixedRotation = True#False
        #bodyDef.angle = b2_pi/180.0 * -20.0
        body = self.bodies["LowerTorso"] = engine.physics_world.CreateBody(bodyDef)
        #bodyDef.angle = b2_pi/180.0 * 0.0
        bodyDef.fixedRotation = False
        polyDef.SetAsBox(0.1, 0.1)
        polyDef.density = 1.0
        body.CreateShape(polyDef)
        polyDef.density = 1.0
        body.SetMassFromShapes()

        revJointDef.Initialize(self.bodies["UpperTorso"], self.bodies["LowerTorso"], refPos + b2Vec2(0.0, 0.1))
        revJointDef.enableLimit = True
        revJointDef.lowerAngle = b2_pi/180.0 * -10.0
        revJointDef.upperAngle = b2_pi/180.0 * 40.0
        self.joints["Abs"] = engine.physics_world.CreateJoint(revJointDef)
        
        bodyDef.position = refPos + b2Vec2(0.0, -0.3)
        body = self.bodies["LeftUpperLeg"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.08, 0.2)
        body.CreateShape(polyDef)
        body.SetMassFromShapes()
        
        revJointDef.Initialize(self.bodies["LowerTorso"], self.bodies["LeftUpperLeg"], refPos + b2Vec2(0.0, -0.1))
        revJointDef.enableLimit = True
        revJointDef.lowerAngle = b2_pi/180.0 * -10.0
        revJointDef.upperAngle = b2_pi/180.0 * 100.0
        self.joints["LeftHip"] = engine.physics_world.CreateJoint(revJointDef)

        bodyDef.position = refPos + b2Vec2(0.0, -0.7)
        body = self.bodies["LeftLowerLeg"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.08, 0.2)
        body.CreateShape(polyDef)
        body.SetMassFromShapes()
        
        revJointDef.Initialize(self.bodies["LeftUpperLeg"], self.bodies["LeftLowerLeg"], refPos + b2Vec2(0.0, -0.5))
        revJointDef.enableLimit = True
        revJointDef.lowerAngle = b2_pi/180.0 * -160.0
        revJointDef.upperAngle = b2_pi/180.0 * 0.0
        self.joints["LeftKnee"] = engine.physics_world.CreateJoint(revJointDef)
        
        bodyDef.position = refPos + b2Vec2(0.075, -0.95)
        body = self.bodies["LeftFoot"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.15, 0.05)
        polyDef.friction = 0.9
        body.CreateShape(polyDef)
        body.SetMassFromShapes()

        revJointDef.Initialize(self.bodies["LeftLowerLeg"], self.bodies["LeftFoot"], refPos + b2Vec2(0.0, -0.9))
        revJointDef.enableLimit = True
        revJointDef.lowerAngle = b2_pi/180.0 * -90.0
        revJointDef.upperAngle = b2_pi/180.0 * 20.0
        self.joints["LeftAnkle"] = engine.physics_world.CreateJoint(revJointDef)
        
        bodyDef.position = refPos + b2Vec2(0.0, -0.3)
        body = self.bodies["RightUpperLeg"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.08, 0.2)
        body.CreateShape(polyDef)
        body.SetMassFromShapes()
        
        revJointDef.Initialize(self.bodies["LowerTorso"], self.bodies["RightUpperLeg"], refPos + b2Vec2(0.0, -0.1))
        revJointDef.enableLimit = True
        revJointDef.lowerAngle = b2_pi/180.0 * -10.0
        revJointDef.upperAngle = b2_pi/180.0 * 100.0
        self.joints["RightHip"] = engine.physics_world.CreateJoint(revJointDef)

        bodyDef.position = refPos + b2Vec2(0.0, -0.7)
        body = self.bodies["RightLowerLeg"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.08, 0.2)
        body.CreateShape(polyDef)
        body.SetMassFromShapes()
        
        revJointDef.Initialize(self.bodies["RightUpperLeg"], self.bodies["RightLowerLeg"], refPos + b2Vec2(0.0, -0.5))
        revJointDef.enableLimit = True
        revJointDef.lowerAngle = b2_pi/180.0 * -160.0
        revJointDef.upperAngle = b2_pi/180.0 * 0.0
        self.joints["RightKnee"] = engine.physics_world.CreateJoint(revJointDef)
        
        bodyDef.position = refPos + b2Vec2(0.075, -0.95)
        body = self.bodies["RightFoot"] = engine.physics_world.CreateBody(bodyDef)
        polyDef.SetAsBox(0.15, 0.05)
        polyDef.friction = 0.9
        body.CreateShape(polyDef)
        body.SetMassFromShapes()

        revJointDef.Initialize(self.bodies["RightLowerLeg"], self.bodies["RightFoot"], refPos + b2Vec2(0.0, -0.9))
        revJointDef.enableLimit = True
        revJointDef.lowerAngle = b2_pi/180.0 * -90.0
        revJointDef.upperAngle = b2_pi/180.0 * 20.0
        self.joints["RightAnkle"] = engine.physics_world.CreateJoint(revJointDef)
        # ...
        
        self.ref_body = self.bodies["LowerTorso"]
        """
        self.models["Head"] = engine.renderer.meshLoader.getMesh("Head"), \
                                    engine.renderer.textureLoader.getTexture("Head.bmp"), \
                                    (1.0, 1.0, 1.0, 1.0))
         # The mesh for this model is generated just prior to rendering
        self.models["Torso"] = Model(None, \
                                    engine.renderer.textureLoader.getTexture("Torso.bmp"), \
                                    (1.0, 1.0, 1.0, 1.0))
        self.models["LeftElbow"] = Model(engine.renderer.meshLoader.getBoxMesh((0.1, 0.3)), \
                                    engine.renderer.textureLoader.getTexture("Elbow.bmp"), \
                                    (1.0, 1.0, 1.0, 1.0))
        """
        renderer = self.engine.renderer
        self.models["LeftUpperLeg"] = engine.renderer.\
            createModel(layer="play", \
                        mesh=renderer.meshLoader.genBoxMesh((0.1, 0.25)), \
                        texture="UpperLeg", \
                        influences=[(self.bodies["LeftUpperLeg"].GetXForm(), 1.0)],\
                        local_xform=b2XForm(b2Vec2(-0.05, 0), b2Mat22(0)), \
                        use_vertex_colors=False, \
                        color=(1.0, 1.0, 1.0, 1.0))
        self.models["LeftLowerLeg"] = engine.renderer.\
            createModel(layer="play", \
                        mesh=renderer.meshLoader.genBoxMesh((0.08, 0.25)), \
                        texture="LowerLeg", \
                        influences=[(self.bodies["LeftLowerLeg"].GetXForm(), 1.0)],\
                        local_xform=b2XForm(b2Vec2(0, -0.02), b2Mat22(0)), \
                        use_vertex_colors=False, \
                        color=(1.0, 1.0, 1.0, 1.0))
        self.models["LeftFoot"] = engine.renderer.\
            createModel(layer="play", \
                        mesh=renderer.meshLoader.genBoxMesh((0.15, 0.05)), \
                        texture="Foot", \
                        influences=[(self.bodies["LeftFoot"].GetXForm(), 1.0)],\
                        local_xform=b2XForm(b2Vec2(0, -0.05), b2Mat22(0)), \
                        use_vertex_colors=False, \
                        color=(1.0, 1.0, 1.0, 1.0))
        self.models["LeftKnee"] = engine.renderer.\
            createModel(layer="play", \
                        mesh=renderer.meshLoader.genBoxMesh((0.09, 0.09)), \
                        texture="Knee", \
                        influences=[(self.bodies["LeftUpperLeg"].GetXForm(), 0.5), \
                                    (self.bodies["LeftLowerLeg"].GetXForm(), 0.5)],\
                        local_xform=b2XForm(b2Vec2(0.1, -0.02), b2Mat22(0)), \
                        use_vertex_colors=False, \
                        color=(1.0, 1.0, 1.0, 1.0))

        self.models["RightUpperLeg"] = engine.renderer.\
            createModel(layer="play", \
                        mesh=renderer.meshLoader.genBoxMesh((0.1, 0.25)), \
                        texture="UpperLeg", \
                        influences=[(self.bodies["RightUpperLeg"].GetXForm(), 1.0)],\
                        local_xform=b2XForm(b2Vec2(-0.05, 0), b2Mat22(0)), \
                        use_vertex_colors=False, \
                        color=(1.0, 1.0, 1.0, 1.0))
        self.models["RightLowerLeg"] = engine.renderer.\
            createModel(layer="play", \
                        mesh=renderer.meshLoader.genBoxMesh((0.08, 0.25)), \
                        texture="LowerLeg", \
                        influences=[(self.bodies["RightLowerLeg"].GetXForm(), 1.0)],\
                        local_xform=b2XForm(b2Vec2(0, -0.02), b2Mat22(0)), \
                        use_vertex_colors=False, \
                        color=(1.0, 1.0, 1.0, 1.0))
        self.models["RightFoot"] = engine.renderer.\
            createModel(layer="play", \
                        mesh=renderer.meshLoader.genBoxMesh((0.15, 0.05)), \
                        texture="Foot", \
                        influences=[(self.bodies["RightFoot"].GetXForm(), 1.0)],\
                        local_xform=b2XForm(b2Vec2(0, -0.05), b2Mat22(0)), \
                        use_vertex_colors=False, \
                        color=(1.0, 1.0, 1.0, 1.0))
        self.models["RightKnee"] = engine.renderer.\
            createModel(layer="play", \
                        mesh=renderer.meshLoader.genBoxMesh((0.09, 0.09)), \
                        texture="Knee", \
                        influences=[(self.bodies["RightUpperLeg"].GetXForm(), 0.5), \
                                    (self.bodies["RightLowerLeg"].GetXForm(), 0.5)],\
                        local_xform=b2XForm(b2Vec2(0.1, -0.02), b2Mat22(0)), \
                        use_vertex_colors=False, \
                        color=(1.0, 1.0, 1.0, 1.0))
        
        self.animation = self.engine.animationLoader.getAnimation("walking")
        #self.animation = self.engine.animationLoader.getAnimation("charleston")
        
        self.animation.speed = 1.0#0.1
        self.animation.start()
        
        #self.killJoint("Abs")
        #self.killJoint("Neck")
        #self.killJoint("Head")
        #self.breakJoint("Abs")
        
        self.enableJointLimit("LeftHip", False)
        self.enableJointLimit("LeftKnee", False)
        self.enableJointLimit("LeftAnkle", False)
        self.enableJointLimit("RightHip", False)
        self.enableJointLimit("RightKnee", False)
        self.enableJointLimit("RightAnkle", False)
        
        maxPower = 6.0
        self.setJointMaxPower("LeftHip", maxPower)
        self.setJointMaxPower("LeftKnee", maxPower)
        self.setJointMaxPower("LeftAnkle", maxPower)
        self.setJointMaxPower("RightHip", maxPower)
        self.setJointMaxPower("RightKnee", maxPower)
        self.setJointMaxPower("RightAnkle", maxPower)
    
    def setJointMaxPower(self, jointName, maxPower):
        """Sets the maximum power, or torque, a joint can apply.
            
            Can be used analogously to muscle strength.
        """
        joint = self.joints[jointName].asRevoluteJoint()
        joint.SetMaxMotorTorque(maxPower)
    
    def rotateJoint(self, jointName, speed):
        """Sets a joint rotating at the given speed.
        """
        joint = self.joints[jointName].asRevoluteJoint()
        joint.SetMotorSpeed(speed)
    
    def rotateJointTo(self, jointName, angle, speedFactor = 1.0):
        """Rotates a joint to a given angle at a given speed.
           
           Can be used to apply physical animation.
        """
        joint = self.joints[jointName].asRevoluteJoint()
        angleError = joint.GetJointAngle() - angle
        joint.SetMotorSpeed(-speedFactor * angleError)
        
    def killJoint(self, jointName):
        """Renders a joint lifeless and floppy.
        """
        joint = self.joints[jointName].asRevoluteJoint()
        joint.EnableMotor(False)
        
    def reviveJoint(self, jointName):
        """Gives a joint the ability to apply torque again.
        """
        joint = self.joints[jointName].asRevoluteJoint()
        joint.EnableMotor(True)
        
    def breakJoint(self, jointName):
        joint = self.joints[jointName].asRevoluteJoint()
        self.engine.physics_world.DestroyJoint(joint)
        joint = None
        self.joints.pop(jointName)
        
    def enableJointLimit(self, jointName, value=True):
        joint = self.joints[jointName].asRevoluteJoint()
        joint.EnableLimit(value)
    
    def update(self, timeStep):
        
        #self.rotateJointTo("LeftAnkle", b2_pi/180.0* 15)
        #self.killJoint("LeftAnkle")
        
        # Note: This is a quick attempt at allowing joints to snap if under too much load.
        # TODO: Clean this up and implement neatly, with no magic numbers
        """for name, joint in self.joints.items():
            if abs(joint.GetReactionTorque()) > 5.0:
                self.engine.physics_world.DestroyJoint(joint)
                joint = None
                self.joints.pop(name)
        """
        
        # TODO: Replace this crap with options for enabling\disabling\setting the different autonomous motion systems
        #return
        
        if self.animation is not None:
            self.animation.update()
            keyframe = self.animation.getCurFrame()
            for joint, angle in keyframe.jointAngles:
                # TODO: Check what speed should be used here
                self.rotateJointTo(joint, b2_pi/180.0 * angle, 1.0 / (keyframe.time / self.animation.speed))
        
        
        # This applies a restoring torque to keep the reference body at a specified angle w.r.t. vertical.
        # (i.e., prevent the character from falling over)
        # It's a bit of a hack. It should be exactly the same as the code behind joint motors, only we
        #  can't use a joint because it would constrain the position as well as rotation. Perhaps we could
        #  make a new joint type which only constrains the relative angles, rather than positions as well.
        tilt = self.ref_body.GetAngle() - b2_pi/180.0 * 0.0
        speed = -tilt * 3.0
        tiltSpeed = self.ref_body.GetAngularVelocity()
        torque = speed - tiltSpeed
        self.ref_body.ApplyTorque(torque)
        
        return
        
        if self.joints.has_key("LeftShoulder"):
            self.rotateJointTo("LeftShoulder", b2_pi/180.0 * 27.0, 2.0)
        if self.joints.has_key("LeftAnkle"):
            self.rotateJointTo("LeftAnkle", b2_pi/180.0 * 20, 2.0)
        if self.joints.has_key("Abs"):
            self.rotateJointTo("Abs", 0.0, 2.0)
        if self.joints.has_key("LeftHip"):
            self.rotateJointTo("LeftHip", b2_pi/180.0 * 55, 2.0)
        if self.joints.has_key("LeftKnee"):
            self.rotateJointTo("LeftKnee", b2_pi/180.0 * -55, 2.0)
        
        # Note: Not currently applying the auto-balancing algorithm
        return
        # Note: Below is an attempt at self-balancing of the character. It is actually quite
        #       effective, considering its simplicity.
        
        angle = self.ref_body.GetAngle()
        #while angle > 2*b2_pi: angle -= 2*b2_pi
        #while angle < -2*b2_pi: angle += 2*b2_pi
        angleError = angle - b2_pi/180.0* 0.0#-30.0
        angleMotion = self.ref_body.GetAngularVelocity()
        
        #angleMotion *= 0.1
        
        """ Note: This was an attempt to use Fuzzy Logic to self-balance
        epsilon = b2_pi/180.0* 1
        small = b2_pi/180.0* 10
        medium = b2_pi/180.0* 90
        result = 0.0
        
        if abs(angleError) < epsilon and abs(angleMotion) < epsilon:
            result = 0.0
        elif angleError > epsilon and angleError < small and \
             angleMotion > epsilon and angleMotion < small:
            result = small
        elif angleError > small and angleError < medium and \
             abs(angleMotion) < epsilon:
            result = medium
        elif angleError > epsilon and angleError < small and \
             angleMotion > -small and angleMotion < -epsilon:
            result = 0.0
        elif angleError > -medium and angleError < -small and \
             abs(angleMotion) < epsilon:
            result = -medium
        elif angleError > -small and angleError < -epsilon and \
             angleMotion > -small and angleMotion < -epsilon:
            result = -small
        elif angleError > -small and angleError < -epsilon and \
             angleMotion > epsilon and angleMotion < small:
            result = 0.0
        
        result *= 1.0
        """
        
        #self.rotateJoint("LeftAnkle", result)
        self.setJointMaxPower("LeftShoulder", 0.1)
        self.rotateJointTo("LeftShoulder", 4.0*(2.0*angleError + 6.0*angleMotion), 0.8)
        self.setJointMaxPower("LeftAnkle", 0.1)
        self.rotateJointTo("LeftAnkle", 1.0*(2.0*angleError + 6.0*angleMotion) + b2_pi/180.0 * 20, 0.8)
        self.setJointMaxPower("Abs", 0.1)
        self.rotateJointTo("Abs", 0.2*(2.0*angleError + 6.0*angleMotion), 0.8)
        self.setJointMaxPower("LeftHip", 0.1)
        self.rotateJointTo("LeftHip", 0.5*(2.0*angleError + 6.0*angleMotion) + b2_pi/180.0 * 55, 0.8)
        self.setJointMaxPower("LeftKnee", 0.1)
        self.rotateJointTo("LeftKnee", 0.5*(2.0*angleError + 6.0*angleMotion) + b2_pi/180.0 * -55, 0.8)
        
        #self.killJoint("LeftAnkle")
        
        #self.rotateJointTo("LeftKnee", b2_pi/180.0* -45)
        #self.rotateJointTo("LeftHip", b2_pi/180.0* 45)
        #self.rotateJointTo("Abs", b2_pi/180.0* 0)
        #self.rotateJointTo("Neck", b2_pi/180.0* 0)
        #self.rotateJointTo("Head", b2_pi/180.0* -10)
        #self.rotateJointTo("LeftShoulder", b2_pi/180.0* 90, 3.0)ww


    # TODO: Remove this completely
    def draw(self):
        return
        CompositeBody.draw(self)
        return
        
        glPushMatrix()
        xform = self.bodies["Head"].GetXForm()
        glMultMatrixf(xform.GetOpenGLForm())
        #self.models["Head"].render()
        glPopMatrix()
        """
        glPushMatrix()
        torso = self.models["Torso"]
        torso.mesh = self.engine.renderer.meshLoader.generateLineMesh(self.joints["Neck"].GetAnchor1(), \
                                                                 self.joints["Abs"].GetAnchor1(), \
                                                                 self.joints["LeftHip"].GetAnchor1())
        
        torso.render()
        glPopMatrix()
        """
        """
        glPushMatrix()
        xform = self.bodies["LeftUpperArm"].GetXForm()
        glMultMatrixf(xform.GetOpenGLForm())
        # TODO: Check how scaling will work
        self.models["LeftUpperArm"].render()
        glPopMatrix()
        
        glPushMatrix()
        # Angle the elbow model half-way between the lower and upper arm angles
        xform = b2XForm(self.joints["LeftElbow"].position,
                        0.5*(self.bodies["LeftUpperArm"].getAngle() + self.bodies["LeftLowerArm"].getAngle()))
        glMultMatrixf(xform.GetOpenGLForm())
        # TODO: Check how scaling will work
        self.models["LeftElbow"].render()
        glPopMatrix()
        """
        
        self.engine.renderDebugBody(self.bodies["LeftUpperArm"])
        self.engine.renderDebugBody(self.bodies["LeftLowerArm"])
        self.engine.renderDebugBody(self.bodies["LeftHand"])
        self.engine.renderDebugBody(self.bodies["Head"])
        self.engine.renderDebugBody(self.bodies["Neck"])
        self.engine.renderDebugBody(self.bodies["UpperTorso"])
        self.engine.renderDebugBody(self.bodies["LowerTorso"])
        self.engine.renderDebugBody(self.bodies["RightUpperArm"])
        self.engine.renderDebugBody(self.bodies["RightLowerArm"])
        self.engine.renderDebugBody(self.bodies["RightHand"])
        
        #glPushMatrix()
        #xform = self.bodies["LeftUpperLeg"].GetXForm()
        #glMultMatrixf(xform.GetOpenGLForm())
        self.models["UpperLeg"].draw()
        #glPopMatrix()
        
        #glPushMatrix()
        #xform = self.bodies["LeftLowerLeg"].GetXForm()
        #glMultMatrixf(xform.GetOpenGLForm())
        #glTranslatef(0.04, 0.0, 0.0)
        self.models["LowerLeg"].draw()
        #glPopMatrix()
        
        if self.joints.has_key("LeftKnee"):
            #glPushMatrix()
            # Angle the elbow model half-way between the lower and upper arm angles
            #angle = 0.5*(self.bodies["LeftUpperLeg"].GetAngle() + self.bodies["LeftLowerLeg"].GetAngle())
            #xform = b2XForm(self.joints["LeftKnee"].GetAnchor1(), b2Mat22(angle))
            #glMultMatrixf(xform.GetOpenGLForm())
            # Tweak by applying a slight further transformation
            #glTranslatef(0.04, 0.04, 0.0)
            # TODO: Check how scaling will work
            self.models["Knee"].draw()
            #glPopMatrix()
        
        #glPushMatrix()
        #xform = self.bodies["LeftFoot"].GetXForm()
        #glMultMatrixf(xform.GetOpenGLForm())
        #glTranslatef(0.06, 0.04, 0.0)
        self.models["Foot"].draw()
        #glPopMatrix()
        
        #glPushMatrix()
        #xform = self.bodies["RightUpperLeg"].GetXForm()
        #glMultMatrixf(xform.GetOpenGLForm())
        self.models["UpperLeg"].draw()
        #glPopMatrix()
        
        #glPushMatrix()
        #xform = self.bodies["RightLowerLeg"].GetXForm()
        #glMultMatrixf(xform.GetOpenGLForm())
        #glTranslatef(0.04, 0.0, 0.0)
        self.models["LowerLeg"].draw()
        #glPopMatrix()
        
        if self.joints.has_key("RightKnee"):
            #glPushMatrix()
            # Angle the elbow model half-way between the lower and upper arm angles
            #angle = 0.5*(self.bodies["RightUpperLeg"].GetAngle() + self.bodies["RightLowerLeg"].GetAngle())
            #xform = b2XForm(self.joints["RightKnee"].GetAnchor1(), b2Mat22(angle))
            #glMultMatrixf(xform.GetOpenGLForm())
            # Tweak by applying a slight further transformation
            #glTranslatef(0.04, 0.04, 0.0)
            # TODO: Check how scaling will work
            self.models["Knee"].draw()
            #glPopMatrix()
        
        #glPushMatrix()
        #xform = self.bodies["RightFoot"].GetXForm()
        #glMultMatrixf(xform.GetOpenGLForm())
        #glTranslatef(0.06, 0.04, 0.0)
        self.models["Foot"].draw()
        #glPopMatrix()
        
        #self.renderDebug()

    def renderDebug(self):
        # Crude rendering of physical shapes for debugging
        for name, body in self.bodies.items():
            self.engine.renderDebugBody(body)


class Level(CompositeBody):
    """Contains all of the content of the game level
       
       Dependencies: xml.dom.minidom
    """
    def __init__(self, engine, name="level"):
        CompositeBody.__init__(self, engine, name)
        self.path = os.path.join(os.getcwd(), "Levels")
    
    def load(self, name):
        file = open(os.path.join(self.path, name + '.xml') )
        xmlDoc = xmlHandler.parse(file)
        xmlOuden = xmlDoc.getElementsByTagName("ouden")[0]
        xmlLevel = xmlOuden.getElementsByTagName("level")[0]
        for xmlGeometry in xmlLevel.getElementsByTagName("solid_geometry"):
            collideChars = xmlGeometry.getAttribute("collideChars")
            collideBullets = xmlGeometry.getAttribute("collideBullets")
            friction = float(xmlGeometry.getAttribute("friction"))
            restitution = float(xmlGeometry.getAttribute("restitution"))
            for xmlPolygon in xmlGeometry.getElementsByTagName("polygon"):
                isClosed = xmlPolygon.getAttribute("closed")
                shapeDef = b2PolygonDef()
                shapeDef.friction = friction
                shapeDef.restitution = restitution
                vertices = []
                for xmlVertex in xmlPolygon.getElementsByTagName("vertex"):
                    vecString = xmlVertex.getAttribute("pos")
                    pos = tuple( [float(x) for x in (vecString.split(","))] )
                    vertices.append(pos)
                shapeDef.setVertices_tuple(tuple(vertices))
                self.engine.physics_world.GetGroundBody().CreateShape(shapeDef)
            for xmlCircle in xmlGeometry.getElementsByTagName("circle"):
                shapeDef = b2CircleDef()
                shapeDef.friction = friction
                shapeDef.restitution = restitution
                vecString = xmlCircle.getAttribute("pos")
                pos = tuple( [float(x) for x in (vecString.split(","))] )
                shapeDef.localPosition = b2Vec2(pos[0], pos[1])
                shapeDef.radius = float(xmlCircle.getAttribute("radius"))
                self.engine.physics_world.GetGroundBody().CreateShape(shapeDef)
        for xmlSensor in xmlLevel.getElementsByTagName("sensor"):
            pass
        for xmlModel in xmlLevel.getElementsByTagName("model"):
            name = xmlModel.getAttribute("name")
            texture_name = xmlModel.getAttribute("texture")
            texture = self.engine.renderer.textureLoader.getTexture(texture_name)
            layer_name = xmlModel.getAttribute("layer")
            xmlMesh = xmlModel.getElementsByTagName("mesh")[0]
            mesh_name, mesh = self.engine.renderer.meshLoader._loadMesh(xmlMesh)
            self.models[name] = self.engine.renderer.\
                                createModel(layer=layer_name, \
                                            mesh=mesh, \
                                            texture=texture, \
                                            local_xform=b2XForm_identity)
        for xmlEntity in xmlLevel.getElementsByTagName("entity"):
            pass

# TODO: Rewrite this to use time.clock() instead of the dodgy pygame timer
class Animation(object):
    class Keyframe(object):
        def __init__(self):
            self.time = 0.0
            self.jointAngles = []
    """Defines a skeletal animation sequence.
    """
    def __init__(self):
        self.name = ""
        self.isLooping = True
        self.speed = 1.0
        self.keyframes = []
        
        self.isRunning = False
        self.frame = 0
        self.lastFrameTime = 0.0
    def getCurFrame(self):
        return self.keyframes[self.frame]
    def start(self):
        #print "starting animation ", self.name
        #for k in self.keyframes:
        #    print k.time
        #    print k.jointAngles
        self.isRunning = True
        self.lastFrameTime = pygame.time.get_ticks()/1000.0
        self.frame = 0
    def update(self):
        if self.isRunning:
            now = pygame.time.get_ticks()/1000.0
            deltaTime = now - self.lastFrameTime
            if deltaTime >= self.keyframes[self.frame].time / self.speed:
                #self.lastFrameTime = now
                self.lastFrameTime += self.keyframes[self.frame].time / self.speed
                self.frame += 1
                if self.frame == len(self.keyframes):
                    if self.isLooping:
                        self.frame = 0
                    else:
                        self.isRunning = False
            #print self.frame
                    

class ResourceError(Error):
    pass
class AnimationLoader(object):
    def __init__(self):
        self.animations = {}
        self.path = os.path.join(os.getcwd(), "Animations")
    
    def loadResources(self):
        """Call on engine init to load animation info from files
        """
        for file in glob.glob( os.path.join(self.path, '*.xml') ):
            xmlDoc = xmlHandler.parse(file)
            xmlOuden = xmlDoc.getElementsByTagName("ouden")[0]
            for xmlAnimation in xmlOuden.getElementsByTagName("animation"):
                self.__loadAnimation(xmlAnimation)
        
        # Also add a special case None animation
        animation = Animation()
        animation.name = "None"
        self.animations[animation.name] = animation
        
    def __loadAnimation(self, xmlAnimation):
        """Loads basic information about an animation and adds it to the registry.
        """
        animation = Animation()
        animation.name = xmlAnimation.getAttribute("name")
        animation.isLooping = xmlAnimation.getAttribute("loopsByDefault")
        for xmlKeyframe in xmlAnimation.getElementsByTagName("keyframe"):
            keyframe = Animation.Keyframe()
            keyframe.time = float(xmlKeyframe.getAttribute("time"))
            for xmlJoint in xmlKeyframe.getElementsByTagName("joint"):
                keyframe.jointAngles.append( (xmlJoint.getAttribute("name"), float(getXMLNodeText(xmlJoint))) )
            animation.keyframes.append(keyframe)
        self.animations[animation.name] = animation
    
    def getAnimation(self, name):
        if name is None:
            return copy.copy(self.animations["None"])
        # Check if animation exists in the registry
        elif self.animations.has_key(name):
            return copy.copy(self.animations[name])
        else:
            raise ResourceError("animation \"" + name + "\" is not known, i.e., no animation with this name was found in any .xml file in " + self.path)

def getXMLNodeText(xmlNode):
    """Returns the text contained within an xml node
    """
    text = ""
    for node in xmlNode.childNodes:
       if node.nodeType == node.TEXT_NODE:
          text += node.data
    return text








